{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "# HybridNonlinearISAM"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/hybrid/doc/HybridNonlinearISAM.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop\n",
        "except ImportError:\n",
        "    pass  # Not running on Colab, do nothing"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "`HybridNonlinearISAM` provides an interface for performing incremental nonlinear optimization for hybrid systems using ISAM (Incremental Smoothing and Mapping). It manages an underlying `HybridGaussianISAM` object and handles the relinearization process.\n",
        "\n",
        "Key concepts:\n",
        "*   Maintains a full nonlinear factor graph (`HybridNonlinearFactorGraph`) of all factors added so far.\n",
        "*   Keeps track of the current linearization point (`Values`) and discrete assignment (`DiscreteValues`).\n",
        "*   Uses an internal `HybridGaussianISAM` object to store the incrementally updated linearized representation (`HybridBayesTree`).\n",
        "*   The `update` method linearizes new nonlinear factors, incorporates them into the `HybridGaussianISAM`, and updates the estimate.\n",
        "*   Periodically (controlled by `reorderInterval`), it can perform a full relinearization and reordering of the underlying `HybridGaussianISAM` to maintain accuracy and efficiency."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {},
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "import graphviz\n",
        "\n",
        "from gtsam import (\n",
        "    HybridNonlinearISAM,\n",
        "    HybridNonlinearFactorGraph,\n",
        "    HybridNonlinearFactor,\n",
        "    PriorFactorPose2,\n",
        "    BetweenFactorPose2,\n",
        "    Pose2,\n",
        "    Point3,\n",
        "    DecisionTreeFactor,\n",
        "    Values,\n",
        ")\n",
        "from gtsam.symbol_shorthand import X, D"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Initialization"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Initialized HybridNonlinearISAM with reorderInterval=0\n"
          ]
        }
      ],
      "source": [
        "# Initialize HybridNonlinearISAM\n",
        "# reorderInterval=1 means re-linearize/re-order every update (like batch)\n",
        "# Set higher (e.g., 10, 100) for true incremental behavior\n",
        "reorder_interval = 0 # Set to 0 to disable reordering for this example\n",
        "hisam = HybridNonlinearISAM(reorder_interval)\n",
        "\n",
        "print(f\"Initialized HybridNonlinearISAM with reorderInterval={hisam.reorderInterval()}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Incremental Updates\n",
        "\n",
        "The `update` method takes new factors (`HybridNonlinearFactorGraph`) and initial estimates (`Values`) for any *new* variables introduced in those factors."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "--- Update 0 ---\n",
            "ISAM state after update 0:\n",
            " Current Lin Point Size: 1\n",
            " Current Estimate 0:\n",
            "Values with 1 values:\n",
            "Value x0: (gtsam::Pose2)\n",
            "(0, 1.73472e-18, -7.77621e-19)\n",
            "\n",
            "\n",
            "--- Update 1 ---\n",
            "ISAM state after update 1:\n",
            " Current Lin Point Size: 2\n",
            " Current Estimate 1:\n",
            "Values with 2 values:\n",
            "Value x0: (gtsam::Pose2)\n",
            "(8.67362e-18, -1.73472e-18, 9.57103e-19)\n",
            "\n",
            "Value x1: (gtsam::Pose2)\n",
            "(1.00005, -6.64995e-07, -7.77621e-19)\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# --- Initial Step (Pose 0 and Mode Prior) ---\n",
        "step0_graph = HybridNonlinearFactorGraph()\n",
        "step0_values = Values()\n",
        "\n",
        "# Add prior on pose X0\n",
        "prior_noise = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.1, 0.1, 0.05))\n",
        "step0_graph.add(PriorFactorPose2(X(0), Pose2(0, 0, 0), prior_noise))\n",
        "step0_values.insert(X(0), Pose2(0.01, 0.01, 0.01)) # Initial estimate for X0\n",
        "\n",
        "# Add prior on discrete mode D0\n",
        "dk0 = (D(0), 2) # Binary mode\n",
        "step0_graph.add(DecisionTreeFactor([dk0], \"0.8 0.2\")) # P(D0=0)=0.8\n",
        "\n",
        "print(\"--- Update 0 ---\")\n",
        "hisam.update(step0_graph, step0_values)\n",
        "print(\"ISAM state after update 0:\")\n",
        "# hisam.print() # Printing can be verbose\n",
        "print(f\" Current Lin Point Size: {hisam.getLinearizationPoint().size()}\")\n",
        "current_estimate_0 = hisam.estimate()\n",
        "print(\" Current Estimate 0:\")\n",
        "current_estimate_0.print()\n",
        "\n",
        "\n",
        "# --- Second Step (Pose 1 and Hybrid Odometry) ---\n",
        "step1_graph = gtsam.HybridNonlinearFactorGraph()\n",
        "step1_values = gtsam.Values()\n",
        "\n",
        "# Add HybridNonlinearFactor for odometry X0->X1\n",
        "# Mode 0 (Grippy): Smaller noise\n",
        "noise0 = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.1, 0.1, np.radians(1)))\n",
        "odom0 = BetweenFactorPose2(X(0), X(1), Pose2(1.0, 0, 0), noise0)\n",
        "# Mode 1 (Slippery): Larger noise\n",
        "noise1 = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.5, 0.5, np.radians(10)))\n",
        "odom1 = BetweenFactorPose2(X(0), X(1), Pose2(1.0, 0, 0), noise1)\n",
        "hybrid_odom = HybridNonlinearFactor(dk0, [odom0, odom1])\n",
        "step1_graph.add(hybrid_odom)\n",
        "\n",
        "# Provide initial estimate for the *new* variable X1\n",
        "# Use odometry from the current estimate of X0\n",
        "x0_estimate = current_estimate_0.atPose2(X(0))\n",
        "x1_initial_guess = x0_estimate.compose(Pose2(1.0, 0, 0))\n",
        "step1_values.insert(X(1), x1_initial_guess)\n",
        "\n",
        "print(\"\\n--- Update 1 ---\")\n",
        "hisam.update(step1_graph, step1_values)\n",
        "print(\"ISAM state after update 1:\")\n",
        "print(f\" Current Lin Point Size: {hisam.getLinearizationPoint().size()}\")\n",
        "current_estimate_1 = hisam.estimate()\n",
        "print(\" Current Estimate 1:\")\n",
        "current_estimate_1.print() # Should be close to (X0=(0,0,0), X1=(1,0,0), D0=0)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Accessing State and Estimates\n",
        "\n",
        "After updates, you can retrieve the current estimate, the linearization point, the underlying Bayes tree, and the accumulated factors."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Final Estimate:\n",
            "Values with 2 values:\n",
            "Value x0: (gtsam::Pose2)\n",
            "(8.67362e-18, -1.73472e-18, 9.57103e-19)\n",
            "\n",
            "Value x1: (gtsam::Pose2)\n",
            "(1.00005, -6.64995e-07, -7.77621e-19)\n",
            "\n",
            "\n",
            "Current Linearization Point (Values):\n",
            "Values with 2 values:\n",
            "Value x0: (gtsam::Pose2)\n",
            "(0.01, 0.01, 0.01)\n",
            "\n",
            "Value x1: (gtsam::Pose2)\n",
            "(1, 9.57103e-19, -7.77621e-19)\n",
            "\n",
            "\n",
            "Current Discrete Assignment:\n",
            "DiscreteValues{7205759403792793600: 0}\n"
          ]
        }
      ],
      "source": [
        "# Get the final MAP estimate (calculates from current BayesTree)\n",
        "final_estimate = hisam.estimate()\n",
        "print(\"\\nFinal Estimate:\")\n",
        "final_estimate.print()\n",
        "\n",
        "# Get the current linearization point (used for the last update)\n",
        "lin_point = hisam.getLinearizationPoint()\n",
        "print(\"\\nCurrent Linearization Point (Values):\")\n",
        "lin_point.print()\n",
        "\n",
        "# Get the current discrete assignment (MPE from the last update)\n",
        "assignment = hisam.assignment()\n",
        "print(\"\\nCurrent Discrete Assignment:\")\n",
        "print(assignment)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "id": "56fda45b",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Underlying HybridGaussianISAM (HybridBayesTree):\n"
          ]
        },
        {
          "data": {
            "image/svg+xml": [
              "<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n",
              "<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n",
              " \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n",
              "<!-- Generated by graphviz version 12.0.0 (0)\n",
              " -->\n",
              "<!-- Title: G Pages: 1 -->\n",
              "<svg width=\"111pt\" height=\"116pt\"\n",
              " viewBox=\"0.00 0.00 110.70 116.00\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
              "<g id=\"graph0\" class=\"graph\" transform=\"scale(1 1) rotate(0) translate(4 112)\">\n",
              "<title>G</title>\n",
              "<polygon fill=\"white\" stroke=\"none\" points=\"-4,4 -4,-112 106.7,-112 106.7,4 -4,4\"/>\n",
              "<!-- 7205759403792793600 -->\n",
              "<g id=\"node1\" class=\"node\">\n",
              "<title>7205759403792793600</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"51.35\" cy=\"-90\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"51.35\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">d0</text>\n",
              "</g>\n",
              "<!-- 8646911284551352320 -->\n",
              "<g id=\"node2\" class=\"node\">\n",
              "<title>8646911284551352320</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"51.35\" cy=\"-18\" rx=\"51.35\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"51.35\" y=\"-12.95\" font-family=\"Times,serif\" font-size=\"14.00\">x0, x1 : d0</text>\n",
              "</g>\n",
              "<!-- 7205759403792793600&#45;&gt;8646911284551352320 -->\n",
              "<g id=\"edge1\" class=\"edge\">\n",
              "<title>7205759403792793600&#45;&gt;8646911284551352320</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M51.35,-71.7C51.35,-64.41 51.35,-55.73 51.35,-47.54\"/>\n",
              "<polygon fill=\"black\" stroke=\"black\" points=\"54.85,-47.62 51.35,-37.62 47.85,-47.62 54.85,-47.62\"/>\n",
              "</g>\n",
              "</g>\n",
              "</svg>\n"
            ],
            "text/plain": [
              "<graphviz.sources.Source at 0x105a70d70>"
            ]
          },
          "execution_count": 6,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "# Get the underlying Gaussian ISAM object (HybridBayesTree)\n",
        "gaussian_isam = hisam.bayesTree()\n",
        "print(\"\\nUnderlying HybridGaussianISAM (HybridBayesTree):\")\n",
        "# gaussian_isam.print()\n",
        "graphviz.Source(gaussian_isam.dot())  # Visualize the BayesTree"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Relinearization and Reordering\n",
        "\n",
        "If `reorderInterval` is greater than 0, ISAM automatically triggers relinearization and reordering periodically. You can also trigger it manually."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 7,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Manually triggering relinearize...\n",
            "Manual relinearization might be implicit or handled by ISAM2 patterns.\n",
            "Typically happens during update() if counters trigger.\n",
            "Reorder Counter: 0\n",
            "Reorder Interval: 0\n"
          ]
        }
      ],
      "source": [
        "# Manually trigger relinearization (if needed, e.g., after large changes)\n",
        "print(\"\\nManually triggering relinearize...\")\n",
        "# This relinearizes *all* factors at the current estimate and rebuilds the Bayes tree\n",
        "try:\n",
        "    # Note: Direct relinearize might not be exposed or might be handled differently\n",
        "    # in the nonlinear wrapper compared to ISAM2. Often done within update loop.\n",
        "    # hisam.reorderRelinearize() # Check if this method exists/works as expected\n",
        "    print(\"Manual relinearization might be implicit or handled by ISAM2 patterns.\")\n",
        "    print(\"Typically happens during update() if counters trigger.\")\n",
        "except Exception as e:\n",
        "    print(f\"Could not call reorderRelinearize: {e}\")\n",
        "\n",
        "# Check counters\n",
        "print(f\"Reorder Counter: {hisam.reorderCounter()}\")\n",
        "print(f\"Reorder Interval: {hisam.reorderInterval()}\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 5
}
